% draw_holes_on_helmet.m
%
% get scalp_xyz
% find center plane ("Pc")
% find intersection of Pc with scalp "lat(0)"
% find Cz
% find intersection of plane "Plr" through Cz, perpendicular to center plane
%  with scalp "lon(0)"
% find points "d_min"/2 from midline ('arc')
% mark points at "dl" intervals along arc starting at "lon(0)"
% find points "d_min" from arc
% repeat previous 2 steps until done
% 
% N.B. the "scalp brik" (vol_mask) is ARI
%          'holes' is a 2*n_rows cell array of n*3 arrays,
%            each of which lists the coordinates of holes in a row:
%            odd #s on left, even on right 

clear
close all
clc

d_row=10; % spacing between rows (mm)
d_src=10; % spacing between source holes (mm)
d_det=10; % spacing between detector holes (mm)
n_front_holes=20;
n_back_holes=15;
n_rows=15;  % # of rows ON EACH SIDE

scalp_file='C:\data\helmet\mri\S503_121409_scalp.mat';

load(scalp_file)

xo=tai.origin(1); % x origin (midline of +acpc brik)

% extract midline slice
%
midslice=squeeze(vol_mask(xo,:,:)); % is mid saggital slice of mask

% extract x & y coords of midline points
%
[x z]=find(midslice==1); % list of midline scalp points
xyz_midline=zeros(length(x),3); % consider midline == xo
xyz_midline(:,1)=x;
xyz_midline(:,3)=z;

% plot midline
%
scatter3(xyz_midline(:,1),xyz_midline(:,2),xyz_midline(:,3),'.')
xlabel('X')
ylabel('Y')
zlabel('Z')
hold on

% find (& plot) "Cz" (highest point on head)
%
CzZ=max(xyz_midline(:,3)); % (1st) highest value along midline
[CzX iCzx]=find(midslice(:,CzZ)==1,1); % find brik x (A-P) index of Cz
CzY=0; % on midline
scatter3(CzX,CzY,CzZ,'r','filled')

% find hole origins (on +midline)
%
prevX=CzX;  % start at Cz
prevY=CzY;  
prevZ=CzZ;
i_prev=iCzx; % remember previous point so we can avoid it!
tmp_Ml=xyz_midline;
itmp=find(tmp_Ml(:,2)>=CzX); % find all points in back of Cz
tmp_Ml(itmp,:)=1000; % and make them a mile away

for i_hole=1:n_front_holes

    % compute distance from previous point (for all points along midline)
    %
    for iFB=1:length(tmp_Ml)
        dx=prevX-tmp_Ml(iFB,1);
        dy=prevY-tmp_Ml(iFB,2);
        dz=prevZ-tmp_Ml(iFB,3);
        distR(iFB)=sqrt(dx^2+dy^2+dz^2);
    end

    % find this row origin
    %
    ddR=distR-d_src; % difference from desired src spacing
    ddR(i_prev)=1000; % make sure we don't choose previous point!
    [a b]=min(abs(ddR));
    
%     fprintf('Current:  %3.0f) %3.0f, %3.0f, %3.0f\n',b,xyz_Cz(b,1),xyz_Cz(b,2),xyz_Cz(b,3))
    xyz_MlO(i_hole,1)=xyz_midline(b,1);
    xyz_MlO(i_hole,2)=xyz_midline(b,2);
    xyz_MlO(i_hole,3)=xyz_midline(b,3);
    tmp_Ml(b,:)=[];
    i_prev=b; % make sure we don't choose prvious point!
    prevX=xyz_MlO(i_hole,1);
    prevY=xyz_MlO(i_hole,2);
    prevZ=xyz_MlO(i_hole,3);
%     fprintf('previous: %3.0f) %3.0f, %3.0f, %3.0f\n',i_prev,prevX,prevY,prevZ)
    scatter3(xyz_MlO(i_hole,1),xyz_MlO(i_hole,2),xyz_MlO(i_hole,3),'r','filled')

end % i_hole +midline

% find hole origins (on -midline)
%
prevX=CzX;  % start at Cz
prevY=CzY;
prevZ=CzZ;
i_prev=iCzx; % remember previous point so we can avoid it!
tmp_Ml=xyz_midline;
itmp=find(tmp_Ml(:,1)<=CzX); % find all points in front of Cz
tmp_Ml(itmp,:)=1000; % and make them a mile away

for i_hole=1:n_back_holes

    % compute distance from previous point (for all points along midline)
    %
    for iFB=1:length(tmp_Ml)
        dx=prevX-tmp_Ml(iFB,1);
        dy=prevY-tmp_Ml(iFB,2);
        dz=prevZ-tmp_Ml(iFB,3);
        distR(iFB)=sqrt(dx^2+dy^2+dz^2);
    end

    % find this row origin
    %
    ddR=distR-d_src; % difference from desired src spacing
    ddR(i_prev)=1000; % make sure we don't choose previous point!
    [a b]=min(abs(ddR));
    
%     fprintf('Current:  %3.0f) %3.0f, %3.0f, %3.0f\n',b,xyz_Cz(b,1),xyz_Cz(b,2),xyz_Cz(b,3))
    xyz_MlO(i_hole,1)=xyz_midline(b,1);
    xyz_MlO(i_hole,2)=xyz_midline(b,2);
    xyz_MlO(i_hole,3)=xyz_midline(b,3);
    tmp_Ml(b,:)=[];
    i_prev=b; % make sure we don't choose prvious point!
    prevX=xyz_MlO(i_hole,1);
    prevY=xyz_MlO(i_hole,2);
    prevZ=xyz_MlO(i_hole,3);
%     fprintf('previous: %3.0f) %3.0f, %3.0f, %3.0f\n',i_prev,prevX,prevY,prevZ)
    scatter3(xyz_MlO(i_hole,1),xyz_MlO(i_hole,2),xyz_MlO(i_hole,3),'r','filled')

end % i_hole -midline

% populate 1st (L&R) rows from Cz
%
holes{1}=xyz_MlO; % !!! close enough for now...
holes{2}=xyz_MlO;

% extract "Cz" plane
%
Cz_slice=squeeze(vol_mask(:,CzX,:));

% extract & plot Cz line
%
[x y]=find(Cz_slice==1);
% x=1:166;
% y=166-x.^2/100;
xyz_Cz=CzX*ones(length(x),3);
xyz_Cz(:,2)=x-xo; % make midline==xo
xyz_Cz(:,3)=y;
scatter3(xyz_Cz(:,1),xyz_Cz(:,2),xyz_Cz(:,3),'.')
axis equal
% axis vis3d
% axis manual

% find row origins (on +Cz plane)
%
prevX=CzX;  % start at Cz
prevY=CzY; % -d_row/2
% iCxY=find(xyz_Cz(:,2)==-d_row/2)
% prevY=xyz_Cz(iCxY,2)
prevZ=CzZ;
i_prev=iCzx; % remember previous point so we can avoid it!
tmp_Cz=xyz_Cz;
itmp=find(tmp_Cz(:,2)<=0);
tmp_Cz(itmp,:)=1000;
for i_row=1:n_rows

    % compute distance from previous point (for all points along Cz line)
    %
    for iLR=1:length(tmp_Cz)
        dx=prevX-tmp_Cz(iLR,1);
        dy=prevY-tmp_Cz(iLR,2);
        dz=prevZ-tmp_Cz(iLR,3);
        distR(iLR)=sqrt(dx^2+dy^2+dz^2);
    end

    % find this row origin
    %
    ddR=distR-d_row; % difference from desired row spacing
    ddR(i_prev)=1000; % make sure we don't choose previous point!
    [a b]=min(abs(ddR));
    
%     fprintf('Current:  %3.0f) %3.0f, %3.0f, %3.0f\n',b,xyz_Cz(b,1),xyz_Cz(b,2),xyz_Cz(b,3))
    xyz_rowO(i_row,1)=xyz_Cz(b,1);
    xyz_rowO(i_row,2)=xyz_Cz(b,2);
    xyz_rowO(i_row,3)=xyz_Cz(b,3);
    if b<=length(tmp_Cz);tmp_Cz(b,:)=[];end
    i_prev=b; % make sure we don't choose previous point!
    prevX=xyz_rowO(i_row,1);
    prevY=xyz_rowO(i_row,2);
    prevZ=xyz_rowO(i_row,3);
%     fprintf('previous: %3.0f) %3.0f, %3.0f, %3.0f\n',i_prev,prevX,prevY,prevZ)
    scatter3(xyz_rowO(i_row,1),xyz_rowO(i_row,2),xyz_rowO(i_row,3),'b','filled')

    % copy row "origin hole" to 'holes'
    %
    holes{i_row}(1,:)=xyz_rowO(i_row,:);
    
end % i_row +Cz

% find row origins (on -Cz plane)
%
prevX=CzX;  % start at Cz
prevY=CzY;  % -d_row/2
prevZ=CzZ;
i_prev=iCzx; % remember previous point so we can avoid it!
tmp_Cz=xyz_Cz;
itmp=find(tmp_Cz(:,2)>=0);
tmp_Cz(itmp,:)=1000;
for i_row=n_rows+1:2*n_rows

    % compute distance from previous point (for all points along Cz line)
    %
    for iLR=1:length(tmp_Cz)
        dx=prevX-tmp_Cz(iLR,1);
        dy=prevY-tmp_Cz(iLR,2);
        dz=prevZ-tmp_Cz(iLR,3);
        distR(iLR)=sqrt(dx^2+dy^2+dz^2);
    end

    % find this row origin
    %
    ddR=distR-d_row; % difference from desired row spacing
    ddR(i_prev)=1000; % make sure we don't choose previous point!
    [a b]=min(abs(ddR));
    
%     fprintf('Current:  %3.0f) %3.0f, %3.0f, %3.0f\n',b,xyz_Cz(b,1),xyz_Cz(b,2),xyz_Cz(b,3))
    xyz_rowO(i_row,1)=xyz_Cz(b,1);
    xyz_rowO(i_row,2)=xyz_Cz(b,2);
    xyz_rowO(i_row,3)=xyz_Cz(b,3);
    tmp_Cz(b,:)=[];
    i_prev=b; % make sure we don't choose previous point!
    prevX=xyz_rowO(i_row,1);
    prevY=xyz_rowO(i_row,2);
    prevZ=xyz_rowO(i_row,3);
%     fprintf('previous: %3.0f) %3.0f, %3.0f, %3.0f\n',i_prev,prevX,prevY,prevZ)
    scatter3(xyz_rowO(i_row,1),xyz_rowO(i_row,2),xyz_rowO(i_row,3),'b','filled')

    % copy row "origin hole" to 'holes'
    %
    holes{i_row}=xyz_rowO(i_row,:);
    
end % i_row -Cz

% reorder rows from left to right
%
xyz_rowO(1:10,:)=flipud(xyz_rowO(1:10,:));

% now start looking for points "d_src" forward of rowO (origin)
% and "d_row" from previous row. 
%
% start at ths row's origin
%
prevX=CzX-15;  % start at Cz
prevY=CzY+tai.origin(2);
prevZ=CzZ+tai.origin(3);

for i_row=3:2:3 %n_rows % start
    for i_hole=2:2
        % 1st find all points (on scalp) <= "d_src" from current point

       rng = [ [prevX-d_row/2 prevX+d_row/2];...
               [prevY-d_src prevY];...
               [prevZ-d_src prevZ] ];

        tmpX1=find(xyz_mask(:,1)+tai.origin(1)>rng(1,1));
        tmpX2=find(xyz_mask(:,1)+tai.origin(1)<rng(1,2));
        tmpY1=find(xyz_mask(:,2)+tai.origin(2)>rng(2,1));
        tmpY2=find(xyz_mask(:,2)+tai.origin(2)<rng(2,2));
        tmpZ1=find(xyz_mask(:,3)+tai.origin(3)>rng(3,1));
        tmpZ2=find(xyz_mask(:,3)+tai.origin(3)<rng(3,2));
        tmpa=intersect(tmpX1,tmpX2);
        tmpb=intersect(tmpa,tmpY1);
        tmpc=intersect(tmpb,tmpY2);
        tmp=tmpc;
        % size(tmp)
        % tmp=intersect(tmp,tmpZ1);
        % size(tmp)
        % tmp=intersect(tmp,tmpZ2);
        % size(tmp)

        % plot selected patch
        %
%         scatter3(xyz_mask(tmpc,2)+tai.origin(2),...
%                  xyz_mask(tmpc,1),...
%                  xyz_mask(tmpc,3)+tai.origin(3),64,'g','filled')
        %     xlabel('X')
        %     ylabel('Y')
        %     zlabel('Z')
        %     hold on

        % now find distance to each point in this patch from both
        % last point in this row and 'parallel' point in previous row
        % 1st from previous point:
        %
    %     fprintf('mask(t
        x=holes{i_row}(i_hole-1,1);
        y=holes{i_row}(i_hole-1,2);
        z=holes{i_row}(i_hole-1,3);
        fprintf('holes xyz: %3.0f, %3.0f, %3.0f\n',x,y,z)
        
        xl=xlim;yl=ylim;zl=zlim;
        plot3([xl(1) xl(2)],[y y],        [z z],'r')
        plot3([x x],        [yl(1) yl(2)],[z z],'g')
        plot3([x x],        [y y],        [zl(1) zl(2)],'r')
        scatter3(x,y,z,64,'y','filled')
        
        x=xyz_mask(tmp(1),2);
        y=xyz_mask(tmp(1),1);
        z=xyz_mask(tmp(1),3);
        y2=x;%+tai.origin(1);
        x2=y+tai.origin(2);
        z2=z+tai.origin(3);
        fprintf('mask  xyz: %3.0f, %3.0f, %3.0f\n',x,y,z)
        fprintf('last  xyz: %3.0f, %3.0f, %3.0f\n',x2,y2,z2)
        xl=xlim;yl=ylim;zl=zlim;
        plot3([xl(1) xl(2)],[y2 y2],        [z2 z2],'r')
        plot3([x2 x2],        [yl(1) yl(2)],[z2 z2],'g')
        plot3([x2 x2],        [y2 y2],        [zl(1) zl(2)],'r')
        scatter3(x2,y2,z2,64,'y','filled')
        
        fprintf('tai origin: %3.0f, %3.0f, %3.0f\n',...
            tai.origin(1),tai.origin(2),tai.origin(3))
        drawnow

return
        dx1=xyz_mask(tmp,2)-holes{i_row}(i_hole-1,1); % +tai.origin(2)
        dy1=xyz_mask(tmp,1)-holes{i_row}(i_hole-1,2);
        dz1=xyz_mask(tmp,3)-holes{i_row}(i_hole-1,3); % +tai.origin(3)
        dist1=sqrt(dx1.^2+dy1.^2+dz1.^2);
        %
        % now from 'parallel' point in previous (but same side) row
        %
        dx2=xyz_mask(tmp,2)-holes{i_row-2}(i_hole,1); % +tai.origin(2)
        dy2=xyz_mask(tmp,1)-holes{i_row-2}(i_hole,2);
        dz2=xyz_mask(tmp,3)-holes{i_row-2}(i_hole,3); % +tai.origin(3)
        dist2=sqrt(dx2.^2+dy2.^2+dz2.^2);

        figure
        plot(dist1)
        hold on
        plot(dist2)

        % find point on scalp patch d_src from last hole & d_row from last row
        %
        hpts=find(dist1==d_src);
        rpts=find(dist2==d_row);
        pnt=intersect(hpts,rpts);
        holes{i_row}(i_hole,:)=xyz_mask(pnt);
    
    end % i_hole
end % i_row upper half